#include "../common/hik_camera.h"
#include "mv3d_rgbd_ros2_interface/srv/point_cloud.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"  // 添加点云消息头文件
#include <pcl/common/common.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/conditional_removal.h>
#include <rclcpp/rclcpp.hpp>
#include <pcl/filters/extract_indices.h> // 用于过滤点云

#include <cmath> // Add this line to include fabs function

using mv3d_rgbd_ros2_interface::srv::PointCloud;
using std::placeholders::_1;
using std::placeholders::_2;
using namespace std;

class PointCloudServer : public rclcpp::Node
{
public:
    PointCloudServer():Node("hik_camera_point_cloud_server")
    {
        RCLCPP_INFO(this->get_logger(), "-------------------- Server start --------------------");
        mkdir("src/mv3d_rgbd_ros2/point_cloud", S_IRWXU);
        strSavePath = "src/mv3d_rgbd_ros2/point_cloud";
        RCLCPP_INFO(this->get_logger(), "[Subscriber] save path: %s", strSavePath.c_str());
        server = this->create_service<PointCloud>("SavePointCloud", std::bind(&PointCloudServer::savePointCloud, this, _1, _2));
        // 创建点云发布者
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/topic/pc", 10);
    }
private:
    rclcpp::Service<PointCloud>::SharedPtr server;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;  // 点云发布者
    string strSavePath;

// Define the helper function for comparing floats
bool pclAlmostEqual(float a, float b, float epsilon = 1e-8) {
    return std::fabs(a - b) < epsilon;
}
    void savePointCloud(const PointCloud::Request::SharedPtr request, const PointCloud::Response::SharedPtr response)
    {
        // 获取帧号
        uint32_t nFrameNum = request->frame_num;
        // 解析图像类型
        uint32_t nMsgPointCloudType = request->pd_type;
        // 获取相机序列号
        string strSerialNum = request->cam_sn;
        pcl::PCDWriter pcdWriter;
        char chFileName[256] = "";
        sensor_msgs::msg::PointCloud2 point_cloud_msg;  // 用于发布的点云消息


        if(nMsgPointCloudType == TYPE_POINT_CLOUD)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr pclPointCloud(new pcl::PointCloud<pcl::PointXYZ>);
            // 把msg消息转化为点云
            pcl::fromROSMsg(request->point_cloud, *pclPointCloud);
            sprintf(chFileName, "%s/[%d]_point_cloud.pcd", strSavePath.c_str(), nFrameNum);
            pcdWriter.write(chFileName, *pclPointCloud, false);
            RCLCPP_INFO(this->get_logger(), "[Server] SN:%s save [%d]_point_cloud.pcd success", strSerialNum.c_str(), nFrameNum);

            RCLCPP_INFO(this->get_logger(), "-------------------- pcl::PointXYZ--------------------");
            // 转换为ROS消息并发布
            pcl::toROSMsg(*pclPointCloud, point_cloud_msg);
            point_cloud_msg.header.frame_id = "map";  // 设置点云的参考系
            point_cloud_msg.header.stamp = this->get_clock()->now();  // 设置时间戳
            publisher_->publish(point_cloud_msg);
        }
        else if(nMsgPointCloudType == TYPE_POINT_CLOUD_WITH_NORMALS)
        {
#if 1



// 创建法向量点云指针
pcl::PointCloud<pcl::PointNormal>::Ptr pclPointCloudWithNormals(new pcl::PointCloud<pcl::PointNormal>);
// 把msg消息转化为法向量点云
pcl::fromROSMsg(request->point_cloud, *pclPointCloudWithNormals);

// 打印原始点云大小
RCLCPP_INFO(this->get_logger(), "Original Point Cloud Size: %zu", pclPointCloudWithNormals->size());

// 创建索引列表，用于存储需要删除的点的索引
std::vector<int> indicesToRemove;
for (size_t i = 0; i < pclPointCloudWithNormals->points.size(); ++i) {
    if (pclAlmostEqual(pclPointCloudWithNormals->points[i].x, 0.0f) &&
        pclAlmostEqual(pclPointCloudWithNormals->points[i].y, 0.0f) &&
        pclAlmostEqual(pclPointCloudWithNormals->points[i].z, 0.0f)) {
        indicesToRemove.push_back(static_cast<int>(i));
        RCLCPP_DEBUG(this->get_logger(), "Marked point at index: %zu for removal due to zero coordinates.", i);
    }
}

// 如果有需要移除的点，则使用ExtractIndices进行过滤
if (!indicesToRemove.empty()) {
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
    inliers->indices = indicesToRemove;

    pcl::ExtractIndices<pcl::PointNormal> extract;
    extract.setInputCloud(pclPointCloudWithNormals);
    extract.setIndices(inliers);
    extract.setNegative(true); // 设置为true表示保留非标记的点（即不等于0的点）
    extract.filter(*pclPointCloudWithNormals);

    RCLCPP_INFO(this->get_logger(), "Removed %zu points with zero coordinates.", indicesToRemove.size());
} else {
    RCLCPP_INFO(this->get_logger(), "No points found with zero coordinates.");
}

// 打印处理后点云大小
RCLCPP_INFO(this->get_logger(), "Filtered Point Cloud Size: %zu", pclPointCloudWithNormals->size());

// 保存过滤后的点云到PCD文件
sprintf(chFileName, "%s/[%d]_point_cloud_with_normals.pcd", strSavePath.c_str(), nFrameNum);
pcdWriter.write(chFileName, *pclPointCloudWithNormals, false);
RCLCPP_INFO(this->get_logger(), "[Server] SN:%s save [%d]_point_cloud_with_normals.pcd success", strSerialNum.c_str(), nFrameNum);

// 转换为ROS消息并准备发布
sensor_msgs::msg::PointCloud2 point_cloud_msg;
pcl::toROSMsg(*pclPointCloudWithNormals, point_cloud_msg);
point_cloud_msg.header.frame_id = "map";
point_cloud_msg.header.stamp = this->get_clock()->now();

// 发布处理后的点云消息
publisher_->publish(point_cloud_msg);
RCLCPP_INFO(this->get_logger(), "-------------------- pcl::PointNormal--------------------");


#endif
#if 0
            pcl::PointCloud<pcl::PointNormal>::Ptr pclPointCloudWithNormals(new pcl::PointCloud<pcl::PointNormal>);

            // 把msg消息转化为法向量点云
            pcl::fromROSMsg(request->point_cloud, *pclPointCloudWithNormals);

            // 保存点云到文件
            sprintf(chFileName, "%s/[%d]_point_cloud_with_normals.pcd", strSavePath.c_str(), nFrameNum);
            pcdWriter.write(chFileName, *pclPointCloudWithNormals, false);
            RCLCPP_INFO(this->get_logger(), "[Server] SN:%s save [%d]_point_cloud_with_normals.pcd success", strSerialNum.c_str(), nFrameNum);

            // 打印调试信息
            RCLCPP_INFO(this->get_logger(), "-------------------- pcl::PointNormal Debug Information --------------------");

            // 打印点云的基本信息
            RCLCPP_INFO(this->get_logger(), "Point cloud data type: pcl::PointNormal");
            RCLCPP_INFO(this->get_logger(), "Number of points in the point cloud: %ld", pclPointCloudWithNormals->points.size());
            RCLCPP_INFO(this->get_logger(), "Is point cloud empty: %s", pclPointCloudWithNormals->empty() ? "Yes" : "No");

            // 打印点云的前几个点的数据
            size_t numPointsToPrint = 5;
            size_t totalPoints = pclPointCloudWithNormals->points.size();
            size_t pointsToPrint = (totalPoints < numPointsToPrint) ? totalPoints : numPointsToPrint;

            RCLCPP_INFO(this->get_logger(), "First %ld points of the point cloud:", pointsToPrint);
            for (size_t i = 0; i < pointsToPrint; ++i) {
                const auto& point = pclPointCloudWithNormals->points[i];
                RCLCPP_INFO(this->get_logger(), "Point %ld: x=%.3f, y=%.3f, z=%.3f, normal_x=%.3f, normal_y=%.3f, normal_z=%.3f", 
                            i, point.x, point.y, point.z, point.normal_x, point.normal_y, point.normal_z);
            }

            // 打印点云的统计信息
            Eigen::Vector4f min_pt, max_pt;
            pcl::getMinMax3D(*pclPointCloudWithNormals, min_pt, max_pt);
            RCLCPP_INFO(this->get_logger(), "Point cloud bounds: X[%.3f, %.3f], Y[%.3f, %.3f], Z[%.3f, %.3f]", 
                        min_pt[0], max_pt[0], min_pt[1], max_pt[1], min_pt[2], max_pt[2]);


            // 转换为ROS消息并发布
            pcl::toROSMsg(*pclPointCloudWithNormals, point_cloud_msg);
            point_cloud_msg.header.frame_id = "map";
            point_cloud_msg.header.stamp = this->get_clock()->now();

            // 打印ROS消息的header信息
            RCLCPP_INFO(this->get_logger(), "ROS message header: frame_id=%s, stamp=%ld.%ld", 
                        point_cloud_msg.header.frame_id.c_str(), 
                        point_cloud_msg.header.stamp.sec, 
                        point_cloud_msg.header.stamp.nanosec);

            // 发布点云消息
            publisher_->publish(point_cloud_msg);
            RCLCPP_INFO(this->get_logger(), "Published point cloud message to topic: %s", publisher_->get_topic_name());
#endif

#if 0
            // pcl::PointCloud<pcl::PointNormal>::Ptr pclPointCloudWithNormals(new pcl::PointCloud<pcl::PointNormal>);
            // // 把msg消息转化为法向量点云
            // pcl::fromROSMsg(request->point_cloud, *pclPointCloudWithNormals);
            // sprintf(chFileName, "%s/[%d]_point_cloud_with_normals.pcd", strSavePath.c_str(), nFrameNum);
            // pcdWriter.write(chFileName, *pclPointCloudWithNormals, false);
            // RCLCPP_INFO(this->get_logger(), "[Server] SN:%s save [%d]_point_cloud_with_normals.pcd success", strSerialNum.c_str(), nFrameNum);
            
            
            // RCLCPP_INFO(this->get_logger(), "-------------------- pcl::PointNormal--------------------");
            // // 转换为ROS消息并发布
            // pcl::toROSMsg(*pclPointCloudWithNormals, point_cloud_msg);
            // point_cloud_msg.header.frame_id = "map";
            // point_cloud_msg.header.stamp = this->get_clock()->now();
            // publisher_->publish(point_cloud_msg);
#endif
        }
        else if(nMsgPointCloudType == TYPE_TEXTURED_POINT_CLOUD)
        {
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclTexturedPointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
            // 把msg消息转化为纹理点云
            pcl::fromROSMsg(request->point_cloud, *pclTexturedPointCloud);
            sprintf(chFileName, "%s/[%d]_textured_point_cloud.pcd", strSavePath.c_str(), nFrameNum);
            pcdWriter.write(chFileName, *pclTexturedPointCloud, false);
            RCLCPP_INFO(this->get_logger(), "[Server] SN:%s save [%d]_textured_point_cloud.pcd success", strSerialNum.c_str(), nFrameNum);
            
            RCLCPP_INFO(this->get_logger(), "-------------------- pcl::PointXYZRGB--------------------");
            // 转换为ROS消息并发布
            pcl::toROSMsg(*pclTexturedPointCloud, point_cloud_msg);
            point_cloud_msg.header.frame_id = "map";
            point_cloud_msg.header.stamp = this->get_clock()->now();
            publisher_->publish(point_cloud_msg);
        }
        else if(nMsgPointCloudType == TYPE_TEXTURED_POINT_CLOUD_WITH_NORMALS)
        {
            pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclTexturedPointCloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
            // 把msg消息转化为带法向量的纹理点云
            pcl::fromROSMsg(request->point_cloud, *pclTexturedPointCloudWithNormals);
            sprintf(chFileName, "%s/[%d]_textured_point_cloud_with_normals.pcd", strSavePath.c_str(), nFrameNum);
            pcdWriter.write(chFileName, *pclTexturedPointCloudWithNormals, false);
            RCLCPP_INFO(this->get_logger(), "[Server] SN:%s save [%d]_textured_point_cloud_with_normals.pcd success", strSerialNum.c_str(), nFrameNum);
            
            RCLCPP_INFO(this->get_logger(), "-------------------- pcl::PointXYZRGBNormal--------------------");
            // 转换为ROS消息并发布
            pcl::toROSMsg(*pclTexturedPointCloudWithNormals, point_cloud_msg);
            point_cloud_msg.header.frame_id = "map";
            point_cloud_msg.header.stamp = this->get_clock()->now();
            publisher_->publish(point_cloud_msg);
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "[Server] SN:%s point cloud type[%d] error...", nMsgPointCloudType);
        }
    }
};

int main(int argc, char *argv[])
{
    // 初始化ros节点
    rclcpp::init(argc, argv);
    auto server = std::make_shared<PointCloudServer>();
    // 调用spin函数，并传入节点对象指针
    rclcpp::spin(server);
    rclcpp::shutdown();
    return 0;
}
